/* ######################################################################### */
/** \file PrintFunctions.hpp
 * \brief	This file contains the function that
 *
 *
 * PROJECT:   DATMO ROS NODE
 *
 * Copyright (c) 2011 CAR - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodriguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2011-06-23
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ         2011-06-23 \n
 *      First .
 *
 ######################################################################### */


#include <math.h>
#include <sys/stat.h>
#include <sys/types.h>
/* ------------------------------------------------------------------------- */
/**	\namespace DATMO
 *
 *  \brief	This is the namespace of the DATMO ROS NODE
 */
/* ------------------------------------------------------------------------- */

namespace DATMO
{
/* ------------------------------------------------------------------------- */
/**	\class PrintFunctions
 *
 *  \brief  This is the class of PrintFunctions function.
 *
 *
 */
/* ------------------------------------------------------------------------- */
class cl_PrintFunctions
{
public:
	/* ------------------------------------------------------------------------- */
	/**	\fn public static void printMatrix ()
	 *
	 *  \brief
	 *
	 *  \param[in]
	 *  \param[in]
	 *
	 *  \param[out] bool
	 *
	 */
  /* ------------------------------------------------------------------------- */
  static void printMatrix(Matrix M,string name, int iteration, string trial_name)
  {
    ofstream file;
    std::stringstream f_name;
    std::stringstream folder_name;


    folder_name << "./trial_" << trial_name.c_str();
    string folder_string = folder_name.str();
    mkdir(folder_string.c_str(),S_IRWXU);

    f_name << folder_string.c_str() << "/" << name.c_str() << iteration << ".rosmat";

    string f_name2 = f_name.str();
    file.open(f_name2.c_str());
    file << M;
    file.close();


  }
  static void printMatrix2(Matrix M,string name, int iteration, string trial_name)
  {
    ofstream file;
    std::stringstream f_name;
    std::stringstream folder_name;


    folder_name << "./trial_" << trial_name.c_str();
    string folder_string = folder_name.str();
    mkdir(folder_string.c_str(),S_IRWXU);

    f_name << folder_string.c_str() << "/" << name.c_str() << iteration << ".rosmat";
    string f_name2 = f_name.str();
    file.open(f_name2.c_str());
    for(int i=1;i<=M.Nrows();i++)
    {
      for(int j=1;j<=M.Ncols();j++)
      {
        file <<M(i,j)<<" ";
      }
      file<<endl;
    }

    file.close();


  }

  /* ------------------------------------------------------------------------- */
  /**	\fn public static void printDynObjList ()
   *
   *  \brief
   *
   *  \param[in]
   *  \param[in]
   *
   *  \param[out] bool
   *
   */
  /* ------------------------------------------------------------------------- */
  static void printDynObjList(cl_DynamicObjects2 DynObj, string dyn_name, int iteration, string trial_name)
  {
    ofstream file;
    std::stringstream f_name;


    std::stringstream folder_name;


    folder_name << "./trial_" << trial_name.c_str();
    string folder_string = folder_name.str();
    mkdir(folder_string.c_str(),S_IRWXU);

    f_name << folder_string.c_str() << "/" << dyn_name.c_str() << iteration << ".roslist";


    string f_name2 = f_name.str();
    file.open(f_name2.c_str());
    file << "List with "  << DynObj.List.size() << "  Objects" << endl;
    for (int i = 0; i< (int)DynObj.List.size();i++)
    {
      file << "Object "<<i<<endl;
      file << "POS:["<<DynObj.List[i].Pos3D.f_x << ",";
      file << DynObj.List[i].Pos3D.f_y << ",";
      file << DynObj.List[i].Pos3D.f_z << "]"<< endl;

    }
    file.close();


  }

};
}
